/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
package wifipositioner;

import java.text.DecimalFormat;
import java.util.ArrayList;
import java.util.Collection;
import java.util.HashMap;
import java.util.Map;
import org.pi4.locutil.GeoPosition;
import org.pi4.locutil.trace.TraceEntry;


public class ModelKNN implements IModelBasedWifiPositioner {

    private final int K;
    private final boolean weighted;

    public ModelKNN(int K, boolean weighted) {
        this.K = K;
        this.weighted = weighted;
    }
    
    @Override
    public ArrayList<PositionCalculationResult> CalculatePosition(Collection<ModelBasedTrace> offlineTraces, Collection<TraceEntry> onlineTraces) {
        
        ArrayList<PositionCalculationResult> resultList = new ArrayList<>();
        
        for (TraceEntry entry : onlineTraces) {

            Map<ModelBasedTrace, Double> KNearestNeighbours = new HashMap<>();

            // Find K nearest neighbours
            for (ModelBasedTrace offline : offlineTraces) {
                double distance = DistanceCalculator.calcDistance(offline, entry, 15);

                double largestNeighbourDistance = 0;
                ModelBasedTrace neighbourWithLargestDistance = null;

                for (Map.Entry<ModelBasedTrace, Double> neighbour : KNearestNeighbours.entrySet()) {
                    if (neighbour.getValue() > largestNeighbourDistance) {
                        largestNeighbourDistance = neighbour.getValue();
                        neighbourWithLargestDistance = neighbour.getKey();
                    }
                }

                if (distance < largestNeighbourDistance || KNearestNeighbours.size() < K) {
                    if (neighbourWithLargestDistance != null && KNearestNeighbours.size() >= K) {
                        KNearestNeighbours.remove(neighbourWithLargestDistance);
                    }
                    KNearestNeighbours.put(offline, distance);
                }
            }

            // Add the weighted factor to each neighbours value
            double totalDistance = 0;
            for (Map.Entry<ModelBasedTrace, Double> neighbour : KNearestNeighbours.entrySet()) {
                totalDistance += Math.pow(neighbour.getValue(),2);
                neighbour.setValue(1/(Math.pow(neighbour.getValue(),2)+0.0000000000000000000001));
            }
            double totalDistanceRelative = 0;
            for (Map.Entry<ModelBasedTrace, Double> neighbour : KNearestNeighbours.entrySet()) {
                double distanceRelative = neighbour.getValue() * totalDistance;
                totalDistanceRelative += distanceRelative;
                neighbour.setValue(distanceRelative);
            }
            for (Map.Entry<ModelBasedTrace, Double> neighbour : KNearestNeighbours.entrySet()) {
                neighbour.setValue(neighbour.getValue()/totalDistanceRelative);
            }
            
            // Set position based on weighted/unweighted K nearest neigbour
            GeoPosition position = new GeoPosition(0, 0, 0);
            double avgSignalStrength = 0;
            double weightFactor;
            for (Map.Entry<ModelBasedTrace, Double> neighbour : KNearestNeighbours.entrySet()) {        
                if(weighted) {
                    weightFactor = neighbour.getValue();
                } else {
                    weightFactor = (double) 1/K;
                }
                
                position.setX(position.getX() + neighbour.getKey().getPosition().getX() * weightFactor);
                position.setY(position.getY() + neighbour.getKey().getPosition().getY() * weightFactor);
                position.setZ(position.getZ() + neighbour.getKey().getPosition().getZ() * weightFactor);
                avgSignalStrength += neighbour.getValue();
            }

            if (KNearestNeighbours.size() > 0) {
                DecimalFormat df = new DecimalFormat();
                df.setMaximumFractionDigits(2);

                PositionCalculationResult result = new PositionCalculationResult(position, entry.getGeoPosition());
                resultList.add(result);
                System.out.printf("Estimated position: %.2f, %.2f True position: %.2f, %.2f, Avg Signal strength: %.2f | Distance: %.2f m\n",
                        position.getX(), position.getY(), entry.getGeoPosition().getX(), entry.getGeoPosition().getY(), avgSignalStrength, position.distance(entry.getGeoPosition()));
            } else {
                //System.out.println("No best entry found.");
            }
        }
    
        return resultList;
    }
}
